/********************************************************************************
*   
*    Copyright (C) GREUAH: http://www.roboticaeducativa.es
*    Depecabot 3.0 project depecabot@depeca.uah.es
*
*********************************************************************************
*
*    This file is part of depecabot3 libs.
*
*    Depecabot3 libs is free software: you can redistribute it and/or modify
*    it under the terms of the GNU General Public License as published by
*    the Free Software Foundation, either version 3 of the License, or
*    (at your option) any later version.
*
*    Depecabot3 libs is distributed in the hope that it will be useful,
*    but WITHOUT ANY WARRANTY; without even the implied warranty of
*    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*    GNU General Public License for more details.
*
*    For a copy of the GNU General Public License see 
*    <http://www.gnu.org/licenses/>
*
********************************************************************************/

#include "Depecabot3.h"

#include <Arduino.h>
#include <Servo.h>
#include "PCD8544.h"

#define MAX_GP2D120 	350 	//rango maximo del sensor en mm
#define MIN_GP2D120 	40 	//rango minimo del sensor en mm
#define MAX_GP2Y021 	800 	//rango maximo del sensor en mm
#define MIN_GP2Y021 	100 	//rango minimo del sensor en mm

const unsigned char logo_depeca [] = {		//Logo para la pantalla de bienvenida
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x0F, 0x03, 0xC3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3,
0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xF3, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0x07, 0x00, 0xC0, 0xF8, 0xFF, 0x7F, 0x1F, 0x1F, 0x9F, 0x9F, 0x9F, 0x9F, 0x9F,
0x9F, 0x9F, 0x9F, 0x9F, 0x9F, 0x1F, 0x1F, 0x9F, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x3F, 0x07,
0x00, 0xC0, 0xFC, 0xFF, 0x7F, 0x03, 0x00, 0xC0, 0xF8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0x3F, 0x07, 0x00, 0x80, 0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F,
0x01, 0x01, 0xE1, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0x39, 0x01, 0x06, 0xE6, 0xF8, 0xF9,
0x39, 0x01, 0x06, 0xF6, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF9, 0xF8, 0xF8,
0xFC, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xCF, 0xC0, 0xC0, 0xCC, 0xCF,
0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xC1, 0xC0, 0xE0, 0xFE, 0xFF, 0xFF, 0xC3, 0xC0, 0xC0,
0xCE, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xCF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
};

unsigned long encoder1Count,encoder2Count;	//Variables para el control de los encoders

unsigned char encoder1Value,encoder2Value,encoder1OldValue,encoder2OldValue;

Depecarduino::Depecarduino():lcd(SCK,MOSI,LCD_DC,LCD_RST,LCD_SCE),encoder1(ENCODER1),encoder2(ENCODER2)	//Inicializacion de objetos en el constructor.
{

}

void Depecarduino::init ()	//Metodo de inicializacion del robot
{
	Serial1.begin(115200);	//Puerto serie bluetooth
	Wire.begin();		//I2C
	encoderInit();		//Configuracion de la interrupcion

	pinMode(MI_A,OUTPUT);	//Configuracion de los pines de los motores
	pinMode(MI_B,OUTPUT);
	pinMode(MD_A,OUTPUT);
	pinMode(MD_B,OUTPUT);
	
	this->lcd.begin(84, 48);	//Configuracion del LCD

	this->lcd.setCursor(20, 0);	//Pantalla de bienvenida
	this->lcd.drawBitmap(logo_depeca, 46, 5);
	this->lcd.setCursor(1, 5);
	this->lcd.print("Depecabot 3.0");
  
	delay(1000);
}

unsigned char Depecarduino::pulsador(unsigned char canal)	//Devuelve el estado del pulsador especificado
{
	unsigned char puerto_pcf,salida;
	
	if(canal>3)
		salida=0;
	else
		if(canal==1)
			salida=~digitalRead(P1)&0x01;	//Negamos
		else
			if(canal==2)
				salida=~digitalRead(P2)&0x01;	//Negamos
			else
				if(canal==3)
					salida=~digitalRead(P3)&0x01;	//Negamos
	return salida;
}

void Depecarduino::motores(int velocidad1,int velocidad2)	//Controla los motores individualmente
{
	if(velocidad1>255)		//Saturacion
		velocidad1=255;
	if(velocidad1<-255)
		velocidad1=-255;
	if(velocidad2>255)
		velocidad2=255;
	if(velocidad2<-255)
		velocidad2=-255;
		
	if(velocidad1>=0)		//Si la velocidad es positiva escribe en la rama A
	{
		analogWrite(MI_A,velocidad1);
		analogWrite(MI_B,0);
	}
	else		//Si la velocidad es negativa escribe en la rama B
	{
		analogWrite(MI_A,0);
		analogWrite(MI_B,-velocidad1);
	}

	if(velocidad2>=0)		//Si la velocidad es positiva escribe en la rama A
	{
		analogWrite(MD_A,velocidad2);
		analogWrite(MD_B,0);
	}
	else		//Si la velocidad es negativa escribe en la rama B
	{
		analogWrite(MD_A,0);
		analogWrite(MD_B,-velocidad2);
	}
}

void Depecarduino::motoresLinealAngular(int velocidadlineal,int velocidadangular)//Control de los motores en velocidad lineal y angular
{
	int velocidad1,velocidad2;
	
	velocidad1=velocidadlineal+velocidadangular;	//Calcula las velocidades individuales
	velocidad2=velocidadlineal-velocidadangular;
	
	motores(velocidad1,velocidad2);	//Las escribe en los motores
}

float Depecarduino::bateria(void)
{
 	float bateria;

	bateria=0.021*(float)analogRead(VBAT);		//A 8.0V el ADC lee 255
	
	return bateria;
}
	

Encoder::Encoder(unsigned char encoderpin)	//Constructor del objeto encoder
{
	this->pin=encoderpin;
}
	
unsigned int Encoder::read()		//Lectura del encoder, las variables son externas, solo enmascara en forma de objeto para dar coherencia.
{
	if(this->pin==ENCODER1)
		return encoder1Count;
	if(this->pin==ENCODER2)
		return encoder2Count;
}

void Encoder::reset()			//Reinicio del encoder
{
	if(this->pin==ENCODER1)
		encoder1Count=0;
	if(this->pin==ENCODER2)
		encoder2Count=0;
}


SIGNAL(PCINT0_vect)	//Interrupción de cambio de nivel
{
	encoder1Value=digitalRead(ENCODER1);
	encoder2Value=digitalRead(ENCODER2);	//Lee los valores actuales de los pines

	if(encoder1Value!=encoder1OldValue)	//Si el pin ha cambiado de estado incrementa su contador
	{
		encoder1Count++;
		encoder1OldValue=encoder1Value;
	}
	if(encoder2Value!=encoder2OldValue)	//Si el pin ha cambiado de estado incrementa su contador
	{
		encoder2Count++;
		encoder2OldValue=encoder2Value;
	}
}

void encoderInit()
{
  PCMSK0 |= 0x01 << 4;	//Habilita interrupción en el pin 8 de arduino
  PCMSK0 |= 0x01 << 7;	//Habilita interrupción en el pin 11 de arduino
  PCICR |= 0x01;	//Habilita interrupcion en cambio de nivel
}

unsigned int Depecarduino::gp2ymm(unsigned char channel)	//Lectura de sensores de distancia tipo GP2Y021
{
 	float distancia;

	distancia = map(analogRead(channel),735,235,MIN_GP2Y021,MAX_GP2Y021);//Formula de conversion de tension a mm.
	
	if (distancia>MAX_GP2Y021)
		distancia=MAX_GP2Y021; //saturamos
	else if (distancia<MIN_GP2Y021)
			distancia=MIN_GP2Y021; //saturamos
	
	return (int)distancia;
}

unsigned int Depecarduino::gp2dmm(unsigned char channel)	//Lectura de sensores de distancia tipo GP2D120
{
 	float distancia;

	distancia = 10*((2914.0 / (analogRead(channel) + 5.0) - 1));//Formula de conversion de tension a mm.
	
	if (distancia>MAX_GP2D120)
		distancia=MAX_GP2D120; //saturamos
	else if (distancia<MIN_GP2D120)
			distancia=MIN_GP2D120; //saturamos
	
	return (int)distancia;
}


